/*  Code designed by Christopher Allum. 

DESCRIPTION: Implements a distance comparison between current movement 
vector and stored vectors in X dimensional space over a moving window

CHANGE LOGS:
prev 2014        Created
1/19/2015        Added a distance threshold
3/28/2015        Realized i was doing the sqrt in the wrong place. Fixed that.
*/

#include "patternList.h"
#include "patternRecogPrints.h"
#define numFeatures 12
#define numPatterns 6
#define numSamples 30
#define scoreWindowSize 44
#define n 1
//=======ADJUSTABLE PARAMETERS============
boolean outputRaw = false;
float outlierThreshold = 2.3;
float distanceThreshold = 2.4;//over this is ignored
//========================================
//DEPENDENT VARIABLES
Pattern* patterns[numPatterns];

int lastOutput = 8;

float results[numPatterns];
int scoreWindowIter = 0;
float scorePlus = 1;
float tempScores[numPatterns];

int scoreWindow[numPatterns][scoreWindowSize]; 
float tempDistances[numSamples];
float distances[numPatterns][numSamples];
float scoringArray[numPatterns][numSamples];
float totalDistance;

//For the inverse bubble sort
  float tempLowest = 9999;
  float tempDistancesSorted[numFeatures];
  int marker = 0;
  
  //10000 means extra
  //11111 means removed after scoring
  //9999 you shouldn't see but means it was bubbled in the bubble sort

void printPattern(int pNum){
  Serial.print("Pattern: ");
  Serial.println(pNum);
  for(int i = 0; i < numSamples; i++){
    for(int j = 0; j < numFeatures; j++){
      Serial.print(patterns[pNum]->at(i,j));
      Serial.print(" ");
    }
    Serial.println();
  }
  Serial.println();
}

void printAllPatterns(){
  Serial.println("PRINTING ALL PATTERNS");
  for(int i = 0; i < numPatterns; i++){
   printPattern(i); 
  }
}

float getAvg(int p, int i){
  float tempAvg = 0;
  for(int j = 0; j < numSamples; j++){//getAvg
        /*Serial.print("j: ");
        Serial.println(j);
        Serial.print("i: ");
        Serial.println(i);
        Serial.print("value being considered: ");
        Serial.println(patterns[p]->at(j,i));*/
        if(patterns[p]->at(j,i) != 999){
//          Serial.println("adding to tempAvg");
          tempAvg = tempAvg + patterns[p]->at(j, i); 
//          Serial.print(tempAvg);
        }
      }
      
      tempAvg = tempAvg / numSamples;
      return tempAvg;
}

float getStd(int p, int i, int avg){
  float std = 0;
  float tempTotal = 0;
      
//      Serial.print("tempAvg: ");
//      Serial.println(tempAvg);
       //find the standard deviation of that column
      for(int j = 0; j < numSamples; j++){//add the differences
        if(patterns[p]->at(j,i) != 999){
          tempTotal = avg - patterns[p]->at(j,i);
          std += tempTotal * tempTotal; 
        }
      }
      std /= numSamples;
      std = sqrt(std);
      return std;
  
}

void initializePatterns(){
  
  patterns[0] = &forward;
  patterns[1] = &backward;
  patterns[2] = &left;
  patterns[3] = &right;
  patterns[4] = &halt;
  patterns[5] = &slowDown;
  /*Serial.println("==============BEFORE==============");
  printAllPatterns();
  for(int p = 0; p < numPatterns; p++){
    for(int i = 0; i < numSamples; i++){
      for(int j = 0; j < numFeatures; j++){
        patterns[p]->attributes[i, i*numSamples + j] = 0;
      } 
    }
  }
  
  Serial.println("==============AFTER==============");
  printAllPatterns();*/
}

void postProcessData(){
  //Serial.println("==============BEFORE==============");
  //printAllPatterns();

    float tempArray[numSamples];  
  float avg = 0;
  float std = 0;
  float tempTotal = 0;

  //remove outliers
  //iterate through the columns of each pattern to determine the avg of that feature
  for(int p = 0; p < numPatterns; p++){//for each gesture
    for(int iter = 0; iter < numSamples; iter++){
      tempArray[iter] = 0;
    }
    for(int i = 0; i < numFeatures; i++){//do for each column
      avg  = getAvg(p, i);
      std = getStd(p, i, avg);
      
//      Serial.print("standard deviation: ");
//      Serial.println(std);
       //if one of the values is beyond some threshold relative to the deviation then replace that row with all 999
      for(int j = 0; j < numSamples; j++){//for each row at that column
    // Serial.print(F("std: "));
      //     Serial.println(std);
      //    Serial.print(F("patterns at j,i: "));
      //    Serial.println(patterns[p]->at(j,i));
      //    Serial.println("==============AFTER==============");
      //    printAllPatterns();
      //    Serial.print("value checknig against: ");
      //    Serial.println(abs((avg - std*outlierThreshold)));
        if (  (patterns[p]->at(j,i)) < ((avg - std*outlierThreshold)) ||
              (patterns[p]->at(j,i)) > ((avg + std*outlierThreshold))  ){
          
          //remove everything in that row
            //you have reached an invalid!
            tempArray[j] = 1; 
          }
        }
      }
      //at this point update based on where was invalid
      for(int i = 0; i < numSamples; i++){
        if(tempArray[i] == 1){    
          for(int j = 0; j < numSamples; j++){
            patterns[p]->attributes[i, i*numSamples + j] = avg;
          }
        }  
      }
    }

 
  
 // Serial.println("==============AFTER==============");
 // printAllPatterns();
}


void writePatternAttrib(float* input){
  patterns[0] = &forward;
  patterns[1] = &backward;
  patterns[2] = &left;
  patterns[3] = &right;
  patterns[4] = &halt;
  patterns[5] = &slowDown;
  //printPattern(0);
  //printAllPatterns();
  //Serial.println("==============BEFORE==============");
  for(int p = 0; p < numPatterns; p++){
    for(int i = 0; i < patterns[p]->numRows; i++){
        /*Serial.print("\tnumSamples: ");
        Serial.print(numSamples);*/
      for(int j = 0; j < patterns[p]->numColumns; j++){
  //use this to test:  patterns[p]->attributes[i,((i*numSamples)+j)] = p*numFeatures*numSamples + i*numSamples + j;//input[i,((p*i*numSamples + i*numSamples)+j)];//input[i,j];
        /*Serial.print("\n\ti: ");
        Serial.print(i);
        Serial.print("\tj: ");
        Serial.print(j);*/
        /*Serial.print("\nValue of attributes: ");
        Serial.print(patterns[p]->attributes[((i*numFeatures)+j)]);
        Serial.print("\tp: ");
        Serial.print(p);        
        Serial.print("\ti: ");
        Serial.print(i);
        Serial.print("\tj: ");
        Serial.print(j);
        Serial.print("\tindex being accessed: ");
        Serial.print(((i*numFeatures)+j));
        Serial.print("\tindex being written from: ");
        Serial.print(p*numFeatures*numSamples + i*numFeatures+j);*/
        patterns[p]->attributes[((i*numFeatures)+j)] = input[p*numFeatures*numSamples + i*numFeatures+j];//input[i,j];
        //Serial.print("\nAFTER: ");
        //Serial.print(patterns[p]->attributes[((i*numFeatures)+j)]);
        
      }
    }
    
  }
  postProcessData();
  //Serial.println("==============AFTER==============");
  //printAllPatterns();
  //printPattern(0); 
 // delay(4000);
}




void printDistances(Pattern* thisPattern, int i){  
  //because of the 2d array i'll just leave this particular print here
  //The rest of the prints are in patternRecogPrints.h
    for(int j = 0; j < thisPattern->numRows; j++){
      Serial.print(F("Point_")); 
      Serial.print(j);
      Serial.print(F("_dist: "));
      Serial.print(distances[i][j]);
      Serial.print(F("\t"));
    }
    Serial.println();
}

void printScoreWindow(){
Serial.println(F("=========ScoreWindow==========="));
  for (int w = 0; w < scoreWindowSize; w++){
    for(int i = 0; i < numPatterns; i++){
      Serial.print(F("P_"));
      Serial.print(i);
      Serial.print(F("_Score:  "));
      Serial.print(scoreWindow[i][w]);
      Serial.print(F("\t"));
    }
    Serial.println();
  }
}
//   0    1        2      3    4    5      6        7        8        9        10          
//[jerkX, jerkY, jerkZ, rotX, rotY, rotZ, emgAmp1, emgAmp2, emg1RMS, emg2RMS, emg1PeakFreq, 
//  11           12            13             14              15            16              17
//emg2PeakFreq, pattern1Score, pattern2Score, pattern3Score, pattern4Score, pattern5Score, pattern6Score
//FFT data 1: 18-141
//FFT data 2: 142-265
void writePAT_ToPrc(float* array){
  array[12] = results[0];
  array[13] = results[1];
  array[14] = results[2];
  array[15] = results[3];
  array[16] = results[4];
  array[17] = results[5];
}



float* getDistance(float* input, Pattern* thisPattern){
//this is comparing distance between rows of a matrix of 1 by 'numInputs' elements and
//another matrix of 'numRows' by 'numColumns' elements
  for(int i = 0; i < thisPattern->numRows; i++){
    totalDistance = 0.0;
    for (int j = 0; j < (thisPattern->numColumns); j++){

  //CDR method, simple distance
      float newDistance = ((input[j] - thisPattern->at(i,j)) )*((input[j] - thisPattern->at(i,j)) );
  //FDR new method attempt, exponential distance
      //double newDistance = sqrt((    (input[j] - thisPattern->at(i,j)) )*(input[j] - thisPattern->at(i,j)) )    *     (input[j] - thisPattern->at(i,j)*(input[j] - thisPattern->at(i,j) ));
      
      totalDistance = totalDistance + newDistance;
    }
    
    //now that you have all the distances,
    totalDistance = sqrt(totalDistance);
    
    //store the value into a global
    tempDistances[i] = totalDistance;
  }
  //Serial.println("bubble sorting");
  //SORT TEMPDISTANCES
  for(int i = 0; i < thisPattern->numRows; i++){ //must iterate through at least the size of the array
      for(int j = 0; j < thisPattern->numRows; j++){ //must iterate through at least the size of the array
        if(tempDistances[j] < tempLowest){
          tempLowest = tempDistances[j];
          marker = j;//so that we can set that element of the array to 0. Like a pop
        }
      }
      tempDistances[marker] = 9999;
      tempDistancesSorted[i] = tempLowest;
      tempLowest = 9999;
  }
  return tempDistancesSorted;
}

void compareData(float* input){//n is number of nearest neighbors to compare to
//Serial.println("comparing data");delay(200);



  //For each column of the input find the distance to that corresponding column in all the rows of the classifiers
  //define each row on the classifier as a point
  
  //find the distance to each point with each column's distance combined
  for(int i = 0; i < numPatterns; i++){
    getDistance(input, patterns[i]);
    for(int k = 0; k < patterns[i]->numRows; k++){
      distances[i][k] = tempDistancesSorted[k];
    }
  }   
 // Serial.println("distances acquired");delay(200);

  //ASSIGN SCORING FOR SORTED DISTANCES
 //first create a scoring array on which we can change numbers as we need to
 //make it at first equal to the distances array
 boolean isSmallest;
 boolean breakTemp = false;
 //first analyze the array
 int distancesWidth = (sizeof(distances) / sizeof(distances[0]));//numColumns (numSamples)
 int distancesHeight = (sizeof(distances[0])/sizeof(float));//numRows  (numPatterns)
  for(int i = 0; i < distancesWidth; i++){
    for(int j = 0; j < distancesHeight; j++){
      scoringArray[i][j] = distances[i][j];
      //if(scoringArray[i][j] == 0) scoringArray[i][j] = 10000; //so that extra room in the array doesn't mess up the score
    }
  }

  int tempDistance = 0;
  for(int i = 0; i < n; i ++){//for each neighbor
  //iterate through the 2d scoring array for each element in the 2d array means 4 for loops:
    for(int j = 0; j < numPatterns; j++){ //for each pattern(meaning each row in distances array)
      for(int k = 0; k < patterns[j]->numColumns; k++){ //for each column of the pattern
        tempDistance = scoringArray[j][k]; //mark current number's row in a temp variable
        isSmallest = true; //assume it is smallest until proven wrong 
        breakTemp = false;
        //Now check to see if that number is smaller than any other in the whole array

        for(int p = 0; p < numPatterns; p++){ //now iterate through the whole array
          for(int q = 0; q < patterns[j]->numRows; q++){
            if(tempDistance > scoringArray[p][q] ){//if that number is smaller for each piece then it is smallest
              isSmallest = false;
              breakTemp = true;
              break;//end this iteration since you have your answer
            }
          }
          if(breakTemp) break;
        }

        //RIP: Here rests the notorious bug that consumed 8 hours of my life in the prime time before CDR
      //either j or k is out of bounds. Figure this out! LOL. THE SIZE OF THE PATTERNS WAS BASED ON INT NOT DOUBLE>>>> GGWP
      //if(isSmallest){
      if(isSmallest && (tempDistance < distanceThreshold) ){ //attempt at only allowing certain scores
      //Serial.println("EMU: MY BONES ARE MELTING HELP ME!!!!");
        //scorePlus = 1.00 / tempDistance * 20;
        patterns[j]->score = patterns[j]->score + scorePlus;  //then add a point to that row's pattern but only if it is below the distance threshold
        //Serial.println(patterns[j]->score);
        scoringArray[j][k] = distanceThreshold + 100;  //and remove that number from future consideration
        // Serial.println("score assigned");delay(200);
        //reset then break
        isSmallest = false;
        break; //make sure you're done comparing for that pattern
        }  
      }
      
    }
    
  }    
  
  //THE MOVING SCORE WINDOW
  //first store the scoring session's scores into the current element of the array;
  for(int i = 0; i < numPatterns; i++){
    scoreWindow[i][scoreWindowIter] = patterns[i]->score;
    //write into the processing array before it's too late!    
    tempScores[i] = patterns[i]->score;
    patterns[i]->score = 0;//it's too late after this point!
  }
  
  if(scoreWindowIter < scoreWindowSize - 1){
    scoreWindowIter++;} //circularly linked queue lol
  else{ 
    scoreWindowIter = 0;
  }

  //Score each classifier based on how many points were chosen 
  for(int i = 0; i < numPatterns; i++){
    results[i] = 0;
  }
   
  //Now add up all values in the score window and call that the total score for a pattern
  //first analyze the score window
  int scoreWindowWidth = sizeof(scoreWindow) / sizeof(scoreWindow[0]);
  int scoreWindowHeight = sizeof(scoreWindow[0]) / sizeof(float);
  
  for(int i = 0; i < scoreWindowWidth; i++){
    for(int j = 0; j < scoreWindowHeight; j++){
      results[i] = results [i] + scoreWindow[i][j];
    }
    
  }   

  //play sound based on results
  lastOutput = outputPatternSound(results, lastOutput);
 
}


